#include "Process.h"
using namespace std;
using namespace cv;



struct Result
{
	double ang;
	Point2f Center;
	double row;
	double col;
	double Score = 0;
	int AngIndex = 0;
};
struct AngleStep
{
	//基础模板参数
	double Szn = 0.;			//    1/(width*height)
	double sumSquare = 0.;		//    灰度值平方求和
	double sum = 0.;			//    灰度值求和
	double Devn = 0.;			//    多项式提前计算
	double Ang = 0.;
	Mat Val;					//    图片数据
};
struct  TPyramid
{
	int Pindex = 0;				//当前金字塔等级
	vector<AngleStep> TPyramidList;
	//不同角度的结果列表
	vector<Result> ResultList;
	//当前金字塔的最大结果
	Result MaxScore;
};
struct Template
{
	int PNum = 0;
	vector<TPyramid> List;

};



__forceinline double GetInteg(Point P1, Point P2, Mat& src)
{
	double val = src.at<double>(P2.y, P2.x);
	if (P1.x == 0 && P1.y == 0)
	{
		return val;
	}
	if (P1.x == 0)
	{
		return val - src.at<double>(P1.y, P2.x);
	}
	if (P1.y == 0)
	{
		return val - src.at<double>(P2.y, P1.x);
	}
	return val + src.at<double>(P1.y, P1.x) - src.at<double>(P1.y, P2.x) - src.at<double>(P2.y, P1.x);
}

void CreateNCCModel(Mat Mod, Template& templateMod)
{
	//模板预设尺寸
	double ModSize = 30;
	//起始角度 单位度
	double startAng = -4;
	//结束角度 单位度
	double endAng = 4;

	if (Mod.rows <= ModSize || Mod.cols <= ModSize)
	{
		return;
	}

	Mat Resiz;
	int Width = Mod.cols;
	int Height = Mod.rows;
	int Num = MIN((int)log2(Width / ModSize), (int)log2(Height / ModSize));  //自动评估金字塔等级

	//底层金字塔角度范围
	vector<double> angList;
	double AngLenth = endAng - startAng;
	if (AngLenth < 2)
	{
		angList.push_back(0);
		//angList.push_back(180);
	}
	else
	{
		for (double i = startAng; i < endAng; i += 4)
		{
			angList.push_back(i);
		}
	}

	vector<Mat> pyImg(Num + 1);
	pyImg[0] = Mod;
	for (size_t i = 1; i < pyImg.size(); i++)
	{
		pyrDown(pyImg[i - 1], pyImg[i]);
	}

	templateMod.PNum = Num;
	for (size_t i = 0; i < (Num + 1); i++)//每层金字塔等级创建
	{
		TPyramid py;
		for (size_t j = 0; j < angList.size(); j++)//每层角度创建
		{
			AngleStep as;
			//double sca = 1.0 / pow(2, i);//缩放系数
			Mat RX = getRotationMatrix2D(Point2f(pyImg[i].cols / 2, pyImg[i].rows / 2), angList[j], 1);
			warpAffine(pyImg[i], Resiz, RX, pyImg[i].size());//Size(Mod.cols * sca, Mod.rows * sca)

			//resize(Resiz, Resiz, Size(), sca, sca);

			double Szn = 1.0 / ((double)Resiz.cols * Resiz.rows);
			double sumSquare = 0.;
			double sum = 0.;
			for (int m = 0; m < Resiz.rows; m++)
			{
				uchar* templ_ptr = Resiz.ptr<uchar>(m);
				for (int n = 0; n < Resiz.cols; n++)
				{
					sumSquare += (double)templ_ptr[n] * templ_ptr[n];
					sum += (double)templ_ptr[n];
				}
			}
			double Devn = sqrt(sumSquare - Szn * sum * sum);
			as.Szn = Szn;
			as.sumSquare = sumSquare;
			as.sum = sum;
			as.Devn = Devn;
			as.Val = Resiz.clone();
			as.Ang = angList[j];
			py.TPyramidList.push_back(as);
		}
		py.Pindex = i;
		templateMod.List.push_back(py);
	}
}

void FindNCCModel(Mat& InPutImage, Template& templateMod, double setScore, Rect& rec, double& score)
{
	vector<Mat> pyImg(templateMod.PNum + 1);
	pyImg[0] = InPutImage;
	for (size_t i = 1; i < pyImg.size(); i++)
	{
		pyrDown(pyImg[i - 1], pyImg[i]);
	}
	Mat srcTmp;
	Mat src;
	int FStart = templateMod.PNum;
	int Fend = -1;
	for (int i = FStart; i > Fend; i--)//金字塔等级循环
	{
		double sca = 1.0 / pow(2, i);//缩放系数
		Mat TImg;
		templateMod.List[i].ResultList.clear();
		if (i == FStart)//最上层金字塔全图计算
		{

			src = pyImg[i];
			Mat SrcSum, SrcSquare;
			integral(src, SrcSum, SrcSquare, CV_64FC1, CV_64FC1);
			//角度循环
			for (size_t j = 0; j < templateMod.List[i].TPyramidList.size(); j++)
			{
				TImg = templateMod.List[i].TPyramidList[j].Val;
				int rows = src.rows - TImg.rows + 1;
				int cols = src.cols - TImg.cols + 1;

				Mat result(rows, cols, CV_32FC1);
				double num = 0., den = 0.;
				for (int k = 0; k < result.rows; k++)
				{
					float* result_ptr = result.ptr<float>(k);                       //因为result的类型是CV_32FC1，所以其对应的指针所指向的类型为float，用uchar和double都会出错
					for (int l = 0; l < result.cols; l++)
					{
						//num = GetRot(TImg, src(Rect(l, k, TImg.cols, TImg.rows)));
						srcTmp = src(Rect(l, k, TImg.cols, TImg.rows));
						num = CorrelationSSE(TImg, srcTmp);
						//num = 0;
						//for (int m = 0; m < TImg.rows; m++)
						//{
						//	uchar* templ_ptr = TImg.ptr<uchar>(m);
						//	uchar* src_ptr = src.ptr<uchar>(k + m);
						//	for (int n = 0; n < TImg.cols; n++)
						//	{
						//		num += (double)templ_ptr[n] * src_ptr[l + n];
						//	}
						//}
						double K1 = GetInteg(Point(l, k), Point(l + TImg.cols, k + TImg.rows), SrcSum);
						double A1 = num - templateMod.List[i].TPyramidList[j].Szn * templateMod.List[i].TPyramidList[j].sum * K1;
						double K2 = templateMod.List[i].TPyramidList[j].Szn * K1 * K1;
						double A2 = templateMod.List[i].TPyramidList[j].Devn * sqrt(GetInteg(Point(l, k), Point(l + TImg.cols, k + TImg.rows), SrcSquare) - K2);
						result_ptr[l] = (float)(A1 / A2);
					}
				}



				double minVal, maxVal;
				Point minPos, maxPos;
				cv::minMaxLoc(result, &minVal, &maxVal, &minPos, &maxPos);

				//Mat colorImg;
				//cvtColor(src, colorImg, COLOR_GRAY2RGB);
				//rectangle(colorImg, Rect(maxPos, maxPos + Point(TImg.cols, TImg.rows)), Scalar(0, 255, 0), 1);


				if (setScore * pow(0.9, i) < maxVal)
				{
					Result res;
					res.ang = templateMod.List[i].TPyramidList[j].Ang;
					res.Center = maxPos / sca;
					res.row = TImg.rows / sca;
					res.col = TImg.cols / sca;
					res.Score = maxVal;
					res.AngIndex = j;

					templateMod.List[i].ResultList.push_back(res);
				}
			}

			//筛选当前结果列表的最大得分
			templateMod.List[i].MaxScore.Score = 0;
			for (size_t t = 0; t < templateMod.List[i].ResultList.size(); t++)
			{
				if (templateMod.List[i].ResultList[t].Score > templateMod.List[i].MaxScore.Score)
				{
					templateMod.List[i].MaxScore = templateMod.List[i].ResultList[t];
				}
			}
			if (templateMod.List[i].ResultList.size() == 0)
			{
				score = 0;
				rec = Rect(0, 0, 0, 0);
				return;
			}

		}
		else
		{
			//将上一层的结果映射到下一层、局部搜索
			///上一层结果
			Result last = templateMod.List[i + 1].MaxScore;
			int index = last.AngIndex;
			TImg = templateMod.List[i].TPyramidList[index].Val;

			int pix = 2;


			Point2f Pa = last.Center * sca - Point2f(pix, pix);
			Point2f Pb = Pa + Point2f(last.col, last.row) * sca + Point2f(pix, pix) * 2;

			//区域限制 防止越界
			Pa.x = Pa.x < 0 ? 0 : Pa.x;
			Pa.y = Pa.y < 0 ? 0 : Pa.y;
			Pb.x = Pb.x >= pyImg[i].cols ? pyImg[i].cols - 1 : Pb.x;
			Pb.y = Pb.y >= pyImg[i].rows ? pyImg[i].rows - 1 : Pb.y;

			Mat Cutimg = pyImg[i](Rect(Pa, Pb));
			int rows = Cutimg.rows - TImg.rows + 1;
			int cols = Cutimg.cols - TImg.cols + 1;
			Mat SrcSum, SrcSquare;
			integral(Cutimg, SrcSum, SrcSquare, CV_64FC1, CV_64FC1);
			Mat result(rows, cols, CV_32FC1);

			double num = 0., den = 0.;
			for (int h = 0; h < result.rows; h++)
			{
				float* result_ptr = result.ptr<float>(h);                       //因为result的类型是CV_32FC1，所以其对应的指针所指向的类型为float，用uchar和double都会出错
				for (int w = 0; w < result.cols; w++)
				{
					////计算卷积
					//num = GetRot(TImg, Cutimg(Rect(w, h, TImg.cols, TImg.rows)));
					srcTmp = Cutimg(Rect(w, h, TImg.cols, TImg.rows));
					num = CorrelationSSE(TImg, srcTmp);
					double _Szn = templateMod.List[i].TPyramidList[index].Szn;
					double _sum = templateMod.List[i].TPyramidList[index].sum;
					double K1 = GetInteg(Point(w, h), Point(w + TImg.cols, h + TImg.rows), SrcSum);
					double A1 = num - _Szn * _sum * K1;
					double K2 = _Szn * K1 * K1;
					double A2 = templateMod.List[i].TPyramidList[index].Devn * sqrt(GetInteg(Point(w, h), Point(w + TImg.cols, h + TImg.rows), SrcSquare) - K2);
					double disp = (float)(A1 / A2);
					result_ptr[w] = disp;
					if (abs(disp) > 1.5)
					{
						int stop = 0;
					}
				}
			}
			double minVal, maxVal;
			Point minPos, maxPos;
			cv::minMaxLoc(result, &minVal, &maxVal, &minPos, &maxPos);
			templateMod.List[i].MaxScore.ang = templateMod.List[i].TPyramidList[index].Ang;
			templateMod.List[i].MaxScore.Center = (Pa + Point2f(maxPos.x, maxPos.y)) / sca;
			templateMod.List[i].MaxScore.row = TImg.rows / sca;
			templateMod.List[i].MaxScore.col = TImg.cols / sca;
			templateMod.List[i].MaxScore.Score = maxVal;
			templateMod.List[i].MaxScore.AngIndex = index;
		}
	}
	Result Pr = templateMod.List[0].MaxScore;
	score = Pr.Score;
	rec = Rect(Pr.Center, Pr.Center + Point2f(Pr.col, Pr.row));
	//
}



int mainncc()
{
	


	Mat templImage = imread("mod.bmp", 0);


	Template templateMod;
	CreateNCCModel(templImage, templateMod);


	namedWindow("show", 0);
	resizeWindow("show", 930, 700);

	for (size_t i = 1; i < 90; i++)
	{
		string res;
		stringstream ss;
		ss << i;
		ss >> res;

		string path = "C:/Users/36497/Desktop/SaveImg/2021年07月10日/Cam1/" + res + ".bmp";
		Mat srcImage = imread(path, 0);

		if (srcImage.cols == 0)
		{
			continue;
		}
		Rect rec;
		double score;
		double time = static_cast<double>(getTickCount());
		FindNCCModel(srcImage, templateMod, 0.3, rec, score);
		time = ((double)getTickCount() - time) / getTickFrequency();
		std::cout << time <<"::::"<< score << std::endl;
		Mat colorImg;
		cvtColor(srcImage, colorImg, COLOR_GRAY2RGB);
		rectangle(colorImg, rec, Scalar(0, 255, 0), 3);

		imshow("show", colorImg);
		waitKey(250);
	}


	system("pause");
}

